/**
 * @function  接受G29发布的/joy话题的数据 通过串口发布给LORA基站
 * 
*备注： 
 * 
 * maker:crp
 * 2020-7-5
 * 
 * 方向盘位于 ch0 范围是 [-1,1] 顺时针为减小，逆时针为增加
 * 离合器位于 ch1 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 油门 位于 ch2 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 刹车 位于 ch3 范围是 [-1,1]  ，默认状态位 -1   踩下到底为1
 * 方向盘上的方位按键   左右通道位于 ch4 范围是 [-1,0，1]  离散值，默认状态位 0   方向左为1 右为 -1
 * 方向盘上的方位按键   上下通道位于 ch5 范围是 [-1,0，1]  离散值，默认状态位 0  方向上为1  下为 -1
 * 
 *  buttons按键 [0]  "X"  按键  默认为 0    按下为 1
 *  buttons按键 [1]  "□" 按键  默认为 0    按下为 1
 *  buttons按键 [2]  "○" 按键  默认为 0    按下为 1
 *  buttons按键 [3]  "△" 按键  默认为 0    按下为 1
 *  buttons按键 [4]  右侧拨片  默认为 0    按下为 1
 *  buttons按键 [5]  左侧拨片 默认为 0    按下为 1
  *  buttons按键 [6]  R2按键  默认为 0    按下为 1
 *  buttons按键 [7]   L2按键  默认为 0    按下为 1
 *  buttons按键 [8]  SHARE按键  默认为 0    按下为 1
 *  buttons按键 [9]   OPTIONS按键  默认为 0    按下为 1
 *  buttons按键 [10]  R3按键  默认为 0    按下为 1
 *  buttons按键 [11]   L3按键  默认为 0    按下为 1
 * 
  * buttons按键 [12]    档位 左前  默认为 0    按下为 1
 *  buttons按键 [13]   档位 左后  默认为 0    按下为 1
 *  buttons按键 [14]   档位 中前  默认为 0    按下为 1
 *  buttons按键 [15]   档位 中后  默认为 0    按下为 1
 *  buttons按键 [16]   档位 右前  默认为 0    按下为 1
 *  buttons按键 [17]   档位 右后  默认为 0    按下为 1
 * 
 *  buttons按键 [19]   按键 “+”  默认为 0    按下为 1
 *  buttons按键 [20]   档位 “-”   默认为 0    按下为 1
 * 
 *  buttons按键 [23]   档位 红色圆圈“左转按键”   默认为 0    按下为 1
 */

#include<iostream>
#include<string>
#include <stdlib.h>
#include <stdio.h>
#include <sstream>
#include <vector>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h> 
#include <std_msgs/String.h>

#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>

#include <serial/serial.h>
#include <std_msgs/String.h>

#include <boost/asio.hpp>                  //包含boost库函数
#include <boost/bind.hpp>
#include <sys/time.h>

#include <iostream>
#include <fstream>
#include <sstream>

using namespace std;
using namespace boost::asio;           //定义一个命名空间，用于后面的读写操作

unsigned int speed_0ffset=10000; //偏移量 10000 把数据变换到0-20000
unsigned int KP=10000; //放大系数
uint32_t frame_counter=0;

serial::Serial ros_ser;

void cmd_vel_callback(const sensor_msgs::Joy::ConstPtr& msg);
int main(int argc,char** argv)
{

	string sub_joy_topic,dev;
	int buad,time_out,hz;

	ros::init(argc, argv, "lora_server");
	ros::NodeHandle n("~");
	 
	n.param<std::string>("sub_joy_topic", sub_joy_topic, "/G29/joy");
	n.param<std::string>("dev", dev, "/dev/ttyUSB0");
	n.param<int>("buad", buad, 115200);
	n.param<int>("time_out", time_out, 900);
	//n.param<int>("hz", hz, 50);

	ROS_INFO_STREAM("sub_joy_topic:   "<<sub_joy_topic);
	ROS_INFO_STREAM("dev:   "<<dev);
	ROS_INFO_STREAM("buad:   "<<buad);
	ROS_INFO_STREAM("time_out:   "<<time_out);
	//ROS_INFO_STREAM("hz:   "<<hz);
	// 开启串口模块
	 try
	 {
	    ros_ser.setPort(dev);
	    ros_ser.setBaudrate(buad);
	    //ros_serial::Timeout to = serial::Timeout::simpleTimeout(1000);
	    serial::Timeout to = serial::Timeout::simpleTimeout(time_out);
	    ros_ser.setTimeout(to);
	    ros_ser.open();
	    ros_ser.flushInput(); //清空缓冲区数据
	 }
	 catch (serial::IOException& e)
	 {
	      ROS_ERROR_STREAM("Unable to open port ");
	      return -1;
	}
	if(ros_ser.isOpen())
	{
	   ros_ser.flushInput(); //清空缓冲区数据
	    ROS_INFO_STREAM("Serial Port opened");
	}
	else
	{
	    return -1;
	}
 
   
//订阅主题command
	// ros::Rate loop_rate(hz);
	 ros::Subscriber command_sub = n.subscribe(sub_joy_topic, 10, cmd_vel_callback);
	 ros::spin();
	 

	std::cout<<" EXIT ..."<<std::endl;
	ros::waitForShutdown();
	ros::shutdown();

	return 1;

}
void cmd_vel_callback(const sensor_msgs::Joy::ConstPtr& joy)
{
  uint8_t data_tem[60];
uint16_t ch0=0,ch1=0,ch2=0,ch3=0,ch4=0,ch5=0,ch6=0,ch7=0;
uint8_t gear=0;  //档位
uint8_t tem1_buttons=0,tem2_buttons=0; 
	ch0=joy->axes[0]*10000+speed_0ffset;
	ch1=joy->axes[1]*10000+speed_0ffset;
	ch2=joy->axes[2]*10000+speed_0ffset;
	ch3=joy->axes[3]*10000+speed_0ffset;
	ch4=joy->axes[4]*10000+speed_0ffset;
	ch5=joy->axes[5]*10000+speed_0ffset;
	
	//识别方向盘的档位
	if(joy->buttons[12]) 		gear =1;
	else if(joy->buttons[13]) 		gear =2;
	else if(joy->buttons[14]) 		gear =3;
	else if(joy->buttons[15]) 		gear =4;
	else if(joy->buttons[16]) 		gear =5;
	else if(joy->buttons[17]) 		gear =6;
	else  gear =0;
	
	tem1_buttons |= joy->buttons[0]<<0;
	tem1_buttons |= joy->buttons[1]<<1;
	tem1_buttons |= joy->buttons[2]<<2;
	tem1_buttons |= joy->buttons[3]<<3;
	tem1_buttons |= joy->buttons[4]<<4;
	tem1_buttons |= joy->buttons[5]<<5;
	tem1_buttons |= joy->buttons[6]<<6;
	tem1_buttons |= joy->buttons[7]<<7;

	tem2_buttons |= joy->buttons[8]<<0;
	tem2_buttons |= joy->buttons[9]<<1;
	tem2_buttons |= joy->buttons[10]<<2;
	tem2_buttons |= joy->buttons[11]<<3;
	tem2_buttons |= joy->buttons[19]<<4;
	tem2_buttons |= joy->buttons[20]<<5;
	tem2_buttons |= joy->buttons[23]<<6;

	ROS_INFO("frame_counter:  %d ",frame_counter);
	ROS_INFO_STREAM("ch0:   "<<ch0<<"  ch1:   "<<ch1<<"  ch2:   "<<ch2<<"  ch3:   "<<ch3<<"  ch4:   "<<ch4<<"  ch5:   "<<ch5);
	ROS_INFO("gear:  %d \t tem1_buttons:  %X    \t tem2_buttons:  %X  ",gear,tem1_buttons,tem2_buttons);
	
	
  unsigned char i,counter=0;
  unsigned char  cmd;
  uint8_t check=0;
  cmd =0x01;
  data_tem[counter++] =0xAE;
  data_tem[counter++] =0xEA;
  data_tem[counter++] =0x0B; //数据长度
  data_tem[counter++] =cmd;
  
data_tem[counter++] =uint8_t((frame_counter>>24)%256); // 
data_tem[counter++] =uint8_t((frame_counter>>16)%256);
data_tem[counter++] =uint8_t(frame_counter>>8%256); // 
data_tem[counter++] =uint8_t((frame_counter)%256);
 
data_tem[counter++] =uint8_t((ch0)/256); // ch0-ch7通道数据
data_tem[counter++] =uint8_t((ch0)%256);
data_tem[counter++] =uint8_t((ch1)/256); // 
data_tem[counter++] =uint8_t((ch1)%256);
data_tem[counter++] =uint8_t((ch2)/256); // 
data_tem[counter++] =uint8_t((ch2)%256);
data_tem[counter++] =uint8_t((ch3)/256); // 
data_tem[counter++] =uint8_t((ch3)%256);
data_tem[counter++] =uint8_t((ch4)/256); // 
data_tem[counter++] =uint8_t((ch4)%256);
data_tem[counter++] =uint8_t((ch5)/256); // 
data_tem[counter++] =uint8_t((ch5)%256);
data_tem[counter++] =uint8_t((ch6)/256); // 
data_tem[counter++] =uint8_t((ch6)%256);
data_tem[counter++] =uint8_t((ch7)/256); // 
data_tem[counter++] =uint8_t((ch7)%256);

data_tem[counter++] =gear;//目前只使用了3个通道 2个通道备用
data_tem[counter++] =tem1_buttons; // 
data_tem[counter++] =tem2_buttons;
data_tem[counter++] =0x00;
data_tem[counter++] =0x00;


  for(i=0;i<counter;i++)
  {
    check+=data_tem[i];
  }
  check+=0xEF;
  check+=0xFE;
  data_tem[counter++] =check; //0xff表示不需要校验码，属于一个特殊字段，方便调试
  data_tem[counter++] =0xEF;
  data_tem[counter++] =0xFE;
   data_tem[2] =counter;
   
     frame_counter++;
	ros_ser.write(data_tem,counter);
}
 
